#pragma config(Sensor, in1,    PotArm,              sensorPotentiometer)
#pragma config(Sensor, in2,    DetectTube,          sensorLineFollower)
#pragma config(Sensor, dgtl1,  EncoderR,            sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  EncoderL,            sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  BaseSonar,           sensorSONAR_cm)
#pragma config(Sensor, dgtl11, ArmUp,               sensorTouch)
#pragma config(Sensor, dgtl12, ArmDown,             sensorTouch)
#pragma config(Motor,  port1,           DriveRB,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port2,           DriveRF,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port3,           DriveLF,       tmotorNormal, openLoop)
#pragma config(Motor,  port4,           ArmRL,         tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port5,           ArmLL,         tmotorNormal, openLoop)
#pragma config(Motor,  port6,           ArmRU,         tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port7,           ArmLU,         tmotorNormal, openLoop)
#pragma config(Motor,  port8,           SuckR,         tmotorNormal, openLoop)
#pragma config(Motor,  port9,           SuckL,         tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port10,          DriveLB,       tmotorNormal, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!
//#include "delta_lib_v3.c" //Main Funtion Library

#define FULL 127
#define autonFULL 60
//-------------------------------------------| MOTOR SHORTCUTS |---------------------------------------
/*Set Arm Speed*/
void setArmSpeed(int speed) {
  motor[ArmLL] = motor[ArmLU] = motor[ArmRL] = motor[ArmRU] = speed;
}

/*Set Suck Speed*/
void setSuckSpeed(int speed) {
  motor[SuckR] = motor[SuckL] = speed;
}

/*Set Drive Speed: Left & Right*/
void setDriveSpeed(int speed) {
  motor[DriveRF] = motor[DriveRB] = motor[DriveLF] = motor[DriveLB] = speed;
}

/*Set Drive Right Speed*/
void setDriveRSpeed(int speed) {
  motor[DriveRF] = motor[DriveRB] = speed;
}

/*Set Drive Left Speed*/
void setDriveLSpeed(int speed) {
  motor[DriveLF] = motor[DriveLB] = speed;
}

/*Kill Drive Train Motors*/
void killdrive() {
  setDriveSpeed(0);
}
/*Kill Arm Motors*/
void killarm() {
  setArmSpeed(0);
}
/*Kill Suck Motors*/
void killsuck() {
  setSuckSpeed(0);
}


//---------------------------------/ Functions Prototypes /--------------------------------------//
//--------------------------------/                        /-------------------------------------//
//-------------------------------/                          /------------------------------------//
/*void drive(int driveSpeed, float r);
void drive_suck(int driveSpeed, int suckSpeed, float r);
void turn_left(int, driveSpeed, float r);
void turn_right(int, driveSpeed, float r);
void raise_arm(int armSpeed, int finalPos);
void lower_arm(int armSpeed, int finalPos);
void suck(int suckSpeed, int msec);
*/
//--/ PID /-------------------------------------------------------/

int startpoint = 0;
int goal_value = startpoint;
int change = -1000;
int k_P = 10;
int k_I = 0;
int k_D = 0;

//--/ Dead Zone /-------------------------------------------------/
//int DeadZone = 10; //dead zone value for joysticks

//--/ Arm Position /----------------------------------------------/
int low_descore;
int low_lock;
int high_descore;
int high_lock;

//--Declare Global Variables--------------------------------------/
/* 'rotations' will be a counter for every 360 encoder clicks */
/* which is one full rotation of the wheel (ie. 2 'rotations' */
/* will equal 720.0 clicks, 2 full rotations of the wheel).   */
const float rotations = 360.0;

//---------------------------------/ Pre Autonomous /-------------------------------------------//
//--------------------------------/                  /------------------------------------------//
//-------------------------------/                    /-----------------------------------------//

//---------------------------------/ Library /-------------------------------------------------//
//--------------------------------/           /------------------------------------------------//
//-------------------------------/             /-----------------------------------------------//

//--| FORWARD |---------------------------------------------
void drive(int driveSpeed, float r)
{
  SensorValue[EncoderR] = 0;    /* Clear the encoders for    */
  SensorValue[EncoderL] = 0;    /* consistancy and accuracy. */
  // While the encoders have not yet met their goal: (r * rotations) ie (3.0 * 360.0) or "three rotations"
  while(SensorValue[EncoderR] < (r * rotations) && SensorValue[EncoderL] < (r * rotations))
  {
    if (abs(SensorValue[EncoderR]) < abs(SensorValue[EncoderL])){
      setDriveRSpeed(driveSpeed);
      setDriveLSpeed(driveSpeed - 1);
    }
    else if (abs(SensorValue[EncoderR]) > abs(SensorValue[EncoderL])){
      setDriveRSpeed(driveSpeed - 1);
      setDriveLSpeed(driveSpeed);

    }
    else if (abs(SensorValue[EncoderR]) == abs(SensorValue[EncoderL])){
      setDriveSpeed(driveSpeed);
    }
  }//while
  killdrive();
}//void
//----------------------------------------------------------------------------------------------------

//--------------------------------------------| FORWARD + SUCK |---------------------------------------------
void drive_suck(int driveSpeed, int suckSpeed, float r)
{
  SensorValue[EncoderR] = 0;    /* Clear the encoders for     */
  SensorValue[EncoderL] = 0;    /* consistancy and accuracy. */
  // While the encoders have not yet met their goal: (r * rotations) ie (3.0 * 360.0) or "three rotations"
  while (SensorValue[EncoderR] < (r * rotations) && SensorValue[EncoderL] < (r * rotations))
  {
    if (abs(SensorValue[EncoderR]) < abs(SensorValue[EncoderL])){
      setDriveRSpeed(driveSpeed);
      setDriveLSpeed(driveSpeed - 1);
      setSuckSpeed(suckSpeed);
    }
    else if (abs(SensorValue[EncoderR]) > abs(SensorValue[EncoderL])){
      setDriveRSpeed(driveSpeed - 20);
      setDriveLSpeed(driveSpeed);
      setSuckSpeed(suckSpeed);
    }
    else if (abs(SensorValue[EncoderR]) == abs(SensorValue[EncoderL])){
      setDriveSpeed(driveSpeed);
      setSuckSpeed(suckSpeed);
    }
  }//while
  killdrive();
  killsuck();
}//void

//--------------------------------------------| FORWARD + ARM |---------------------------------------------
void drive_arm(int driveSpeed, int armSpeed, float r)
{
  SensorValue[EncoderR] = 0;    /* Clear the encoders for     */
  SensorValue[EncoderL] = 0;    /* consistancy and accuracy. */
  // While the encoders have not yet met their goal: (r * rotations) ie (3.0 * 360.0) or "three rotations"
  while (SensorValue[EncoderR] < (r * rotations) && SensorValue[EncoderL] < (r * rotations))
  {
    if (abs(SensorValue[EncoderR]) < abs(SensorValue[EncoderL])){
      setDriveRSpeed(driveSpeed);
      setDriveLSpeed(driveSpeed - 1);
      setArmSpeed(armSpeed);
    }
    else if (abs(SensorValue[EncoderR]) > abs(SensorValue[EncoderL])){
      setDriveRSpeed(driveSpeed - 20);
      setDriveLSpeed(driveSpeed);
      setArmSpeed(armSpeed);
    }
    else if (abs(SensorValue[EncoderR]) == abs(SensorValue[EncoderL])){
      setDriveSpeed(driveSpeed);
      setArmSpeed(armSpeed);
    }
  }//while
  killdrive();
  killarm();

}//void

//--------------------------------------------| Arc |---------------------------------------------
void drive_arc(int rightSpeed, int leftSpeed, float r)
{
  SensorValue[EncoderR] = 0;    /* Clear the encoders for    */
  SensorValue[EncoderL] = 0;    /* consistancy and accuracy. */

  // While the encoders have not yet met their goal: (r * rotations) ie (3.0 * 360.0) or "three rotations"
  while(SensorValue[EncoderR] < (r * rotations) && SensorValue[EncoderL] < (r * rotations))
  {
    if (SensorValue[EncoderR] < (r * rotations) && SensorValue[EncoderL] < (r * rotations))
      setDriveRSpeed(rightSpeed);
    setDriveLSpeed(leftSpeed);
  }
  killdrive();
}
//-------------------------------------------| TURN LEFT |--------------------------------------------
void turn_left(int driveSpeed, float r)
{
  SensorValue[EncoderR] = 0;    /* Clear the encoders for    */
  SensorValue[EncoderL]  = 0;    /* consistancy and accuracy. */

  // While the encoders have not yet met their goal: (left is compared negativly since it will in reverse)
  while(SensorValue[EncoderR] < (r * rotations) && SensorValue[EncoderL] > (-1 * r * rotations))
  {
    setDriveRSpeed(driveSpeed);
    setDriveLSpeed(-driveSpeed);
  }
  killdrive();
}
//-------------------------------------------| TURN RIGHT |-------------------------------------------
void turn_right(int driveSpeed, float r)
{
  SensorValue[EncoderR] = 0;    /* Clear the encoders for    */
  SensorValue[EncoderL]  = 0;    /* consistancy and accuracy. */

  // While the encoders have not yet met their goal: (left is compared negativly since it will in reverse)
  while(SensorValue[EncoderL] < (r * rotations) && SensorValue[EncoderR] > (-1 * r * rotations))
  {
    setDriveRSpeed(-driveSpeed);
    setDriveLSpeed(driveSpeed);


  }
  killdrive();
}
//-------------------------------------------| MOVE ARM |---------------------------------------------
void move_arm(int armSpeed, int target)
{
  //Upper limit is 610, lower limit is 990
  //While the pot value is greater than +5 of the target, or less than -5 of the target...

  while(SensorValue(PotArm) > target + 5 || SensorValue(PotArm) < target - 5)
  {
    //If the pot value is greater than the target...move the arm up
    if (SensorValue(PotArm) > target)
    {
      setArmSpeed(-armSpeed);
      //motor[port5] = (speed) + 35;
      //motor[port6] = (speed * -1) + 35;
    }
    //If the pot value is less than the target...move the arm down
    if (SensorValue(PotArm) < target)
    {
      setArmSpeed(armSpeed);
      //motor[port5] = (speed * -1) + 35;
      //motor[port6] = (speed) + 35;
    }
  }
  setArmSpeed(0);
  //motor[port5] = 15;
  //motor[port6] = -15;
}
//-------------------------------------------| RAISE ARM |---------------------------------------------
void raise_arm(int armSpeed, int finalPos)
{
  if (SensorValue[PotArm] < finalPos){
    motor[ArmRU] = armSpeed;
    motor[ArmRL] = armSpeed;
    motor[ArmLU] = armSpeed;
    motor[ArmLL] = armSpeed;
  }
  else {/*Zero Arm Motors*/
    motor[ArmRU] = 0;
    motor[ArmRL] = 0;
    motor[ArmLU] = 0;
    motor[ArmLL] = 0;
  }
}
//-------------------------------------------| LOWER ARM |---------------------------------------------
void lower_arm(int armSpeed, int finalPos)
{
  if (SensorValue[PotArm] > finalPos) {
    motor[ArmRU] = -armSpeed;
    motor[ArmRL] = -armSpeed;
    motor[ArmLU] = -armSpeed;
    motor[ArmLL] = -armSpeed;
  }
  else {/*Zero Arm Motors*/
      killarm();
  }
}

void arm_msec(int armSpeed, int msec){
  setArmSpeed(armSpeed);
  wait1msec(msec);
  setArmSpeed(0);
}

//-------------------------------------------| SUCK |--------------------------------------------------
void Suck(int suckSpeed, int msec) {
  setSuckSpeed(suckSpeed);
  wait1Msec(msec);
  killsuck();
}
//-------------------------------------------| Dead Zone |-------------------------------------------
/*int checkDeadZone(int x) {
if (abs(x) < DeadZone) {
return 0;
} else {
if (x < 0)
return (x + DeadZone)*(FULL / (FULL - DeadZone)
else
return (x - DeadZone)*(FULL / (FULL - DeadZone)
}
}
*/
void pre_auton()
{
  //--/ Encoders /-------------------------/
  SensorValue[EncoderL] = 0;
  SensorValue[EncoderR] = 0;

  //--/ Arm Points /-----------------------/
  //goal_value = startpoint + change;
  startpoint/*arm_grounded*/ = SensorValue[PotArm];  // sets ground point           (0 inches)
  low_descore = startpoint + 1556 - 1236;          // sets low descore arm point  (4.5 inches)
  low_lock = startpoint + 2265 - 1236;             //...lowgoal                   (15 inches)
  high_descore = startpoint + 1879 - 1247;          //...high descore              (x inches)
  high_lock = startpoint + 2599 - 1247;            // ...high goal                (18.5 inches)

  // All activities that occur before the competition starts
  // Example: clearing encoders, setting servo positions, ...
}

//---------------------------------/ Autonomous /-----------------------------------------------//
//--------------------------------/              /----------------------------------------------//
//-------------------------------/                /---------------------------------------------//

task autonomous()
{
  // Creates an infinite loop, since "true" always evaluates to true
  {
    drive_suck(autonFULL, -FULL, 4.0);//drive forward and inhale redstack, scoring cheater tube
    suck(-127,750);
    wait1Msec(300);
    turn_right(autonFULL, 1.05);
    arm_msec(FULL, 1000);
    drive_arm(autonFULL, 15, 1.8);
    wait1Msec(1000);
    suck(127, 3000);
    //drive_suck(-autonFULL, FULL, -0.5);

    /* wait1Msec(500);
    drive(autonFULL, 3.0);
    wait1Msec(500);
    move_arm(FULL, low_lock);
    wait1Msec(500);
    drive(autonFULL, 3.0);
    wait1Msec(500);
    move_arm(FULL, low_descore);
    wait1Msec(500);
    drive_suck(autonFULL, FULL, 3.0);//drive forward and inhale redstack, scoring cheater tube

    */
    //drive_suck(autonFULL, -FULL, 3.0);//drive forward and inhale redstack, scoring cheater tube
    //move_arm(FULL, low_lock);//raise arm
    //turn_left(autonFULL, 3.0);//turn to face goal
    //drive(autonFULL, 3.0);// drive to goal
    //move_arm(FULL, 1556);//score tubes
    //drive_suck(autonFULL, FULL, 3.0);//back up while exhaling
    //turn_left(autonFULL, 3.0);//turn around to face blue stack
    //drive_suck(autonFULL, FULL, 3.0);//drive to and inhale blue stack
    //arc(-autonFULL, -FULL, 3.0);// drive back wards in an arc to tower
    //turn_left(autonFULL, 3.0);// turn left away put parallel to tower
    //drive(-autonFULL, 3.0);// back up, parallel to tower
    //turn_right(autonFULL, 3.0);// turn right to face tower
    //move_arm(127, low_lock);//raise arm
    //drive(autonFULL, 3.0);//drive up against tower
    //suck(FULL,1500);//spit tubes into tower
  }

}//task auto

//---------------------------------/ User Control /---------------------------------------------//
//--------------------------------/                /--------------------------------------------//
//-------------------------------/                  /-------------------------------------------//

task usercontrol()
{
  // User control code here, inside the loop
  //pre_auton();
  while (true)
  {

    //--/ Manual_Arm /------------------------------/
    //if(checkDeadZone(vexRT[Ch3]) != 0) {
    if (vexRT[Ch3] > 15 || (vexRT[Ch3]) < -15){
      setArmSpeed(vexRT[Ch3]);
    }
    else {setArmSpeed(0);
    }

    //--/ INHALE /----------------------------------/


    motor[SuckL] = motor[SuckR] = (vexRT[Btn5U] - vexRT[Btn5D]) * FULL;

    //--/ Drive_Train /---------------------------/
    setDriveRSpeed((vexRT[Ch2] - vexRT[Ch1]));// (y - x)
    setDriveLSpeed((vexRT[Ch2] + vexRT[Ch1]));// (y + x)
  }
}
